#include "btsocket.h"


// {B62C4E8D-62CC-404b-BBBF-BF3E3BBB1374}
DEFINE_GUID(g_guidServiceClass, 0xb62c4e8d, 0x62cc, 0x404b, 0xbb, 0xbf, 0xbf, 0x3e, 0x3b, 0xbb, 0x13, 0x74);

#define CXN_TEST_DATA_STRING              ("~!@#$%^&*()-_=+?<>1234567890abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ")
#define CXN_TRANSFER_DATA_LENGTH          (sizeof(CXN_TEST_DATA_STRING))


#define CXN_BDADDR_STR_LEN                17   // 6 two-digit hex values plus 5 colons
#define CXN_MAX_INQUIRY_RETRY             3
#define CXN_DELAY_NEXT_INQUIRY            15
#define CXN_SUCCESS                       0
#define CXN_ERROR                         1
#define CXN_DEFAULT_LISTEN_BACKLOG        4



char g_szRemoteName[BTH_MAX_NAME_SIZE + 1] = { 0 };  // 1 extra for trailing NULL character
char g_szRemoteAddr[CXN_BDADDR_STR_LEN + 1] = { 0 }; // 1 extra for trailing NULL character
int  g_ulMaxCxnCycles = 1;

SOCKET  LocalSocket = INVALID_SOCKET;
//
// NameToBthAddr converts a bluetooth device name to a bluetooth address,
// if required by performing inquiry with remote name requests.
// This function demonstrates device inquiry, with optional LUP flags.
//

ULONG NameToBthAddr(_In_ const LPSTR pszRemoteName, _Out_ PSOCKADDR_BTH pRemoteBtAddr)
{
    INT             iResult = CXN_SUCCESS;
    BOOL            bContinueLookup = FALSE, bRemoteDeviceFound = FALSE;
    ULONG           ulFlags = 0, ulPQSSize = sizeof(WSAQUERYSET);
    HANDLE          hLookup = NULL;
    PWSAQUERYSET    pWSAQuerySet = NULL;

    ZeroMemory(pRemoteBtAddr, sizeof(*pRemoteBtAddr));

    pWSAQuerySet = (PWSAQUERYSET)HeapAlloc(GetProcessHeap(),
        HEAP_ZERO_MEMORY,
        ulPQSSize);
    if (NULL == pWSAQuerySet) {
        iResult = STATUS_NO_MEMORY;
        wprintf(L"!ERROR! | Unable to allocate memory for WSAQUERYSET\n");
    }

    //
    // Search for the device with the correct name
    //
    if (CXN_SUCCESS == iResult) {

        for (INT iRetryCount = 0;
            !bRemoteDeviceFound && (iRetryCount < CXN_MAX_INQUIRY_RETRY);
            iRetryCount++) {
            //
            // WSALookupService is used for both service search and device inquiry
            // LUP_CONTAINERS is the flag which signals that we're doing a device inquiry.
            //
            ulFlags = LUP_CONTAINERS;

            //
            // Friendly device name (if available) will be returned in lpszServiceInstanceName
            //
            ulFlags |= LUP_RETURN_NAME;

            //
            // BTH_ADDR will be returned in lpcsaBuffer member of WSAQUERYSET
            //
            ulFlags |= LUP_RETURN_ADDR;

            if (0 == iRetryCount) {
                wprintf(L"*INFO* | Inquiring device from cache...\n");
            }
            else {
                //
                // Flush the device cache for all inquiries, except for the first inquiry
                //
                // By setting LUP_FLUSHCACHE flag, we're asking the lookup service to do
                // a fresh lookup instead of pulling the information from device cache.
                //
                ulFlags |= LUP_FLUSHCACHE;

                //
                // Pause for some time before all the inquiries after the first inquiry
                //
                // Remote Name requests will arrive after device inquiry has
                // completed.  Without a window to receive IN_RANGE notifications,
                // we don't have a direct mechanism to determine when remote
                // name requests have completed.
                //
                wprintf(L"*INFO* | Unable to find device.  Waiting for %d seconds before re-inquiry...\n", CXN_DELAY_NEXT_INQUIRY);
                Sleep(CXN_DELAY_NEXT_INQUIRY * 1000);

                wprintf(L"*INFO* | Inquiring device ...\n");
            }

            //
            // Start the lookup service
            //
            iResult = CXN_SUCCESS;
            hLookup = 0;
            bContinueLookup = FALSE;
            ZeroMemory(pWSAQuerySet, ulPQSSize);
            pWSAQuerySet->dwNameSpace = NS_BTH;
            pWSAQuerySet->dwSize = sizeof(WSAQUERYSET);
            iResult = WSALookupServiceBegin(pWSAQuerySet, ulFlags, &hLookup);

            //
            // Even if we have an error, we want to continue until we
            // reach the CXN_MAX_INQUIRY_RETRY
            //
            if ((NO_ERROR == iResult) && (NULL != hLookup)) {
                bContinueLookup = TRUE;
            }
            else if (0 < iRetryCount) {
                wprintf(L"=CRITICAL= | WSALookupServiceBegin() failed with error code %d, WSAGetLastError = %d\n", iResult, WSAGetLastError());
                break;
            }

            while (bContinueLookup) {
                //
                // Get information about next bluetooth device
                //
                // Note you may pass the same WSAQUERYSET from LookupBegin
                // as long as you don't need to modify any of the pointer
                // members of the structure, etc.
                //
                // ZeroMemory(pWSAQuerySet, ulPQSSize);
                // pWSAQuerySet->dwNameSpace = NS_BTH;
                // pWSAQuerySet->dwSize = sizeof(WSAQUERYSET);
                if (NO_ERROR == WSALookupServiceNext(hLookup,
                    ulFlags,
                    &ulPQSSize,
                    pWSAQuerySet)) {

                    //
                    // Compare the name to see if this is the device we are looking for.
                    //
                    if ((pWSAQuerySet->lpszServiceInstanceName != NULL) &&
                        (CXN_SUCCESS == _stricmp(pWSAQuerySet->lpszServiceInstanceName, pszRemoteName))) {
                        //
                        // Found a remote bluetooth device with matching name.
                        // Get the address of the device and exit the lookup.
                        //
                        CopyMemory(pRemoteBtAddr,
                            (PSOCKADDR_BTH)pWSAQuerySet->lpcsaBuffer->RemoteAddr.lpSockaddr,
                            sizeof(*pRemoteBtAddr));
                        bRemoteDeviceFound = TRUE;
                        bContinueLookup = FALSE;
                    }
                }
                else {
                    iResult = WSAGetLastError();
                    if (WSA_E_NO_MORE == iResult) { //No more data
                        //
                        // No more devices found.  Exit the lookup.
                        //
                        bContinueLookup = FALSE;
                    }
                    else if (WSAEFAULT == iResult) {
                        //
                        // The buffer for QUERYSET was insufficient.
                        // In such case 3rd parameter "ulPQSSize" of function "WSALookupServiceNext()" receives
                        // the required size.  So we can use this parameter to reallocate memory for QUERYSET.
                        //
                        HeapFree(GetProcessHeap(), 0, pWSAQuerySet);
                        pWSAQuerySet = (PWSAQUERYSET)HeapAlloc(GetProcessHeap(),
                            HEAP_ZERO_MEMORY,
                            ulPQSSize);
                        if (NULL == pWSAQuerySet) {
                            wprintf(L"!ERROR! | Unable to allocate memory for WSAQERYSET\n");
                            iResult = STATUS_NO_MEMORY;
                            bContinueLookup = FALSE;
                        }
                    }
                    else {
                        wprintf(L"=CRITICAL= | WSALookupServiceNext() failed with error code %d\n", iResult);
                        bContinueLookup = FALSE;
                    }
                }
            }

            //
            // End the lookup service
            //
            WSALookupServiceEnd(hLookup);

            if (STATUS_NO_MEMORY == iResult) {
                break;
            }
        }
    }

    if (NULL != pWSAQuerySet) {
        HeapFree(GetProcessHeap(), 0, pWSAQuerySet);
        pWSAQuerySet = NULL;
    }

    if (bRemoteDeviceFound) {
        iResult = CXN_SUCCESS;
    }
    else {
        iResult = CXN_ERROR;
    }

    return iResult;
}

ULONG BthSocketInit() {
    ULONG       ulRetCode = CXN_SUCCESS;
    WSADATA     WSAData = { 0 };
    SOCKADDR_BTH RemoteBthAddr = { 0 };

    //
    // Ask for Winsock version 2.2.
    //
    if (CXN_SUCCESS == ulRetCode) {
        ulRetCode = WSAStartup(MAKEWORD(2, 2), &WSAData);
        if (CXN_SUCCESS != ulRetCode) {
            printf("-FATAL- | Unable to initialize Winsock version 2.2\n");
        }
    }
    return 1;
}
ULONG BthSocketConnectTo(_In_ SOCKADDR_BTH RemoteAddr, _In_ int iMaxCxnCycles) {
    ULONG           ulRetCode = CXN_SUCCESS;
    int             iCxnCount = 0;

    SOCKADDR_BTH    SockAddrBthServer = RemoteAddr;
    HRESULT         res;

 

    if (CXN_SUCCESS == ulRetCode) {
        //
        // Setting address family to AF_BTH indicates winsock2 to use Bluetooth sockets
        // Port should be set to 0 if ServiceClassId is spesified.
        //
        SockAddrBthServer.addressFamily = AF_BTH;
        SockAddrBthServer.serviceClassId = g_guidServiceClass;
        SockAddrBthServer.port = 0;
    }

    if (CXN_SUCCESS == ulRetCode) {
        //
        // Run the connection/data-transfer for user specified number of cycles
        //
        for (iCxnCount = 0;
            (0 == ulRetCode) && (iCxnCount < iMaxCxnCycles || iMaxCxnCycles == 0);
            iCxnCount++) {

            printf("\n");

            //
            // Open a bluetooth socket using RFCOMM protocol
            //
            LocalSocket = socket(AF_BTH, SOCK_STREAM, BTHPROTO_RFCOMM);
            if (INVALID_SOCKET == LocalSocket) {
                printf("=CRITICAL= | socket() call failed. WSAGetLastError = [%d]\n", WSAGetLastError());
                ulRetCode = CXN_ERROR;
                break;
            }

            //
            // Connect the socket (pSocket) to a given remote socket represented by address (pServerAddr)
            //
            if (SOCKET_ERROR == connect(LocalSocket,
                (struct sockaddr*) & SockAddrBthServer,
                sizeof(SOCKADDR_BTH))) {
                printf("=CRITICAL= | connect() call failed. WSAGetLastError=[%d]\n", WSAGetLastError());
                ulRetCode = CXN_ERROR;
                break;
            }

            LocalSocket = INVALID_SOCKET;

        }
    }

    if (INVALID_SOCKET != LocalSocket) {
        closesocket(LocalSocket);
        LocalSocket = INVALID_SOCKET;
    }

    return(ulRetCode);
}

ULONG BthSocketSend(_In_ const LPSTR _data, _In_ int _len) {
    ULONG ulRetCode = CXN_SUCCESS;
    //
    // send() call indicates winsock2 to send the given data
    // of a specified length over a given connection.
    //
    printf("*INFO* | Sending following data string:\n%s\n", _data);
    if (SOCKET_ERROR == send(LocalSocket,
        (char*)_data,
        (int)_len,
        0)) {
        printf("=CRITICAL= | send() call failed w/socket = [0x%I64X], szData = [%p], dataLen = [%I64u]. WSAGetLastError=[%d]\n"
            , (ULONG64)LocalSocket, _data, (ULONG64)_len, WSAGetLastError());
        ulRetCode = CXN_ERROR;
        return 0;
    }
    return 1;
}
ULONG BthSocketClose() {

    if (INVALID_SOCKET != LocalSocket) {
        closesocket(LocalSocket);
        LocalSocket = INVALID_SOCKET;
    }
    return 1;
}